#include "Test.h"
#include<pcl/visualization/pcl_visualizer.h>
#include<iostream>//
#include<pcl/io/io.h>
#include<pcl/io/pcd_io.h>//
#include "comprocess.h"
#include "config.h"
#include<pcl/registration/icp.h>
#include <pcl/filters/voxel_grid.h>
#include<fstream>
#include <ctime>
using namespace std;
using namespace pcl;
typedef pcl::PointXYZ PointIoType;

Test::Test()
{
}


Test::~Test()
{
}

void Test::MappingWithIcpTest()
{
	//Comprocess mComprocess;
	////bin2pcd if you need 
	///*char bin_inpput_path[200] = BIN_INPUT_PATH;
	//if (mComprocess.executebin2pcd(bin_inpput_path)) {
	//	return 0;
	//};*/
	////if you use the code bin2pcd above,please annotation the remaining code below
	//vector<Eigen::Matrix4d> transform;
	//vector<string> pcd_filename;
	//mComprocess.transfromMatrix_Reader(transform);
	//if (transform.empty()) {
	//	return ;
	//}
	//pcd_filename = mComprocess.listFiles(PCD_OUTPUT_PATH);
	//if (pcd_filename.empty()) {
	//	return ;
	//}
	//boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer1(new pcl::visualization::PCLVisualizer("3D Viewer"));
	//pcl::PointCloud<PointIoType>::Ptr map(new pcl::PointCloud<PointIoType>());
	//pcl::PointCloud<PointIoType>::Ptr target(new pcl::PointCloud<PointIoType>());
	//pcl::PointCloud<PointIoType>::Ptr source(new pcl::PointCloud<PointIoType>());	
	//string load_path;
	//for (int i =0; i <1100;i+=10) {
	//	load_path = PCD_OUTPUT_PATH + pcd_filename[i];
	//	//load pcd to a pointcloud name map
	//	if (pcl::io::loadPCDFile(load_path, *source) == -1)
	//		return ;
	//	//transform the pointcloud pose according the transform to the cloud_transfrom
	//	
	//	pcl::transformPointCloud(*source, *target, transform[i]);
	//	
	//	//add the cloud_transfrom to map 
	//	*map = *map + *target;
	//	cout << "process the " << i << "th pointcloud" << endl;
	//}
	////down sampling the points cloud to decrease the memeory
	//pcl::VoxelGrid<pcl::PointXYZ> filter;
	//filter.setInputCloud(map);
	//filter.setLeafSize(0.1f, 0.1f, 0.1f);
	//filter.filter(*map);
	////save the map
	//pcl::io::savePCDFileASCII(PCD_MAP_PATH, *map);
	////show the map
	//viewer1->addPointCloud<PointIoType>(map, "map");
	//viewer1->removePointCloud("source");
	//viewer1->setBackgroundColor(0, 0, 0);
	//while (!viewer1->wasStopped())
	//{
	//	viewer1->spinOnce(100);
	//	boost::this_thread::sleep(boost::posix_time::microseconds(100000));
	//}
}
